/*
--------------------------------------------------------------------------------
				Ak-Lib. 
			 Project       	: Ak-Lib
			 Group         	: Ak-Lib R&D Team
			 Author        	: Ocean.Wang
			 Create Date 	: 05-01-2010
--------------------------------------------------------------------------------
Code Name 	:  A.K 	~v~
Description	:  Yet Another Cross-Platform Lib.
Cautions    	:
--------------------------------------------------------------------------------
Change History:
Version		: 0.1
Building 		: 05/01/10
10-05.01 first draft, build, debug and test.
--------------------------------------------------------------------------------
*/
#include "ak_clk.h"
#include "ak_queue.h"
#include "ak_lock.h"
#include "ak_task.h"

static 	int 	_gTraceLvl_SClk = TRACELVL_WARNING;

/* ---------------------------------------------------------------------------
**	Clock & Trigger Task
*/
#define _STASK_NAME_CLK 		"Ak-SClk"
#define _STASK_STACK_CLK 		0X100000

#define _STASK_NAME_TRIG 		"Ak-STrig"
#define _STASK_STACK_TRIG 		0X100000

static 	_t_sysTask 				_gTask_SClk, _gTask_STrig;

/* Linux */
#if 	_TARGET_OS == _OS_GNU
#define _STASK_PRI_CLK 			90
#define _STASK_PRI_TRIG 		80
/* Windows */
#elif 	_TARGET_OS == _OS_WIN
#define _STASK_PRI_CLK 			THREAD_PRIORITY_TIME_CRITICAL
#define _STASK_PRI_TRIG 		THREAD_PRIORITY_HIGHEST
#endif


/* ---------------------------------------------------------------------------
**	OP Functions & Marcos: Hi-Res CPU TimeStamp
*/
#define _STICK_MAX	 		40960
#define _FREQ_US_SCLK 		5000.0

typedef struct{
	/* Time Stamp */
	_t_timestamp 	curr;
	_t_timestamp 	last;
	_t_timestamp 	freq;

	/* The Cycle Tick */
	uInt32 			tick;
	uInt32 			reserv;
	/* Ratio & Diff */
	sInt64 			tkDiff;
	double 			usDiff;
	double 			usRatio;
}_t_tsCtx;
static 	_t_tsCtx 	_gSTs_Ctx;

int				STs_GetTick(void)
{
	return 	_gSTs_Ctx.tick;
}

/* Linux */
#if 	_TARGET_OS == _OS_GNU
#define _NANO_SEC 				1000000000
#define _NANO_US_VAL 			1000000
#define _NANO_US_RATIO 			(double)(1000000.0/1000000000.0)

#define _TIMEFREQ_GET  			_NANO_US_VAL
#define _TIMEBASE_GET  			_STs_Calc
#define _TS_UPDATE() 			_STs_Calc(NULL)

static 	struct timespec 		_gSTs_Base;

/*
Because of PowerManagement & Stepping CPU Freq, We temp Giveup the method to GetCPU Hi-Res Time Stamp Our Own.
On Linux, We Prefer to use clock_gettime with CLOCK_REALTIME_HR, based on nanosecond. maybe 100ns?
*/
static __inline 	uInt64 _STs_Calc(_t_timestamp * pTs)
{
	register unsigned long long lValue;
	struct timespec  tSpec;

	lValue = 0;
	if(clock_gettime(CLOCK_REALTIME, &tSpec) == 0){
		/* We Calc the Value by nanosecond */
		if(tSpec.tv_nsec < _gSTs_Base.tv_nsec){
			tSpec.tv_sec -= (_gSTs_Base.tv_sec+1);
			tSpec.tv_nsec = (_NANO_SEC +  tSpec.tv_nsec - _gSTs_Base.tv_nsec);
		}else{
			tSpec.tv_sec -= (_gSTs_Base.tv_sec);
			tSpec.tv_nsec = (tSpec.tv_nsec - _gSTs_Base.tv_nsec);
		}

		lValue = (tSpec.tv_sec * _NANO_US_VAL) + tSpec.tv_nsec * _NANO_US_RATIO;
		_gSTs_Ctx.curr.QuadPart = lValue;
		if(pTs)
			pTs->QuadPart = lValue;
	}
	
	return _gSTs_Ctx.curr.QuadPart;
}

/* Windows */
#elif 	_TARGET_OS == _OS_WIN
#include "mmsystem.h" 
#pragma  comment(lib,"winmm.lib")

#define _TIMEBASE_GET  		QueryPerformanceCounter
#define _TIMEFREQ_GET  		QueryPerformanceFrequency

#endif


/* ---------------------------------------------------------------------------
** Clock Common Hi-Res TimerStamp Clock Excursion & Adjust Proc
*/
static 	void 	_STs_Exec(_t_tsCtx * pTsCtx)
{
	uInt64 tsDiff;
	
/* Linux */
#if 	_TARGET_OS == _OS_GNU
	pTsCtx->freq.QuadPart = _NANO_US_VAL;
/* Windows */
#elif 	_TARGET_OS == _OS_WIN
	_TIMEFREQ_GET(&(pTsCtx->freq));
#endif
	pTsCtx->usRatio = (double)(1000000.0)/(double)(pTsCtx->freq.QuadPart);

	/* Get the TimeBase */
	_TIMEBASE_GET(&(pTsCtx->curr));

	/* Calc the Diff Clk TimeStamp */
	tsDiff = pTsCtx->curr.QuadPart - pTsCtx->last.QuadPart;
	pTsCtx->usDiff += 	(double)(tsDiff)*(pTsCtx->usRatio) - _FREQ_US_SCLK;

	/* Calc the Diff Us & Tick */
	pTsCtx->tkDiff	+= (sInt64)((pTsCtx->usDiff)/_FREQ_US_SCLK);	
	SYS_TRACE(_gTraceLvl_SClk, TRACELVL_DEBUG, "TS Calc:[%llu]->[%llu] Diff[%lld].[%f]\n", 
				pTsCtx->curr.QuadPart, 
				pTsCtx->last.QuadPart, 
				pTsCtx->tkDiff, 
				pTsCtx->usDiff);
	
	/* Update the Diff Us & TimeStamp*/		
	pTsCtx->usDiff  	-= ((pTsCtx->tkDiff) * _FREQ_US_SCLK);		
	pTsCtx->last.QuadPart = pTsCtx->curr.QuadPart;

	/* Update the Tick */	
	if(pTsCtx->tkDiff >= 0){	
		pTsCtx->tick	+=	(uInt32)((pTsCtx->tkDiff+1)*_FREQ_MS_SCLK);	
		pTsCtx->tick	%=	(_STICK_MAX);	
		pTsCtx->tkDiff 	= 	0;					
	}else{
		/* Lapse this Tick */	
		SYS_TRACE(_gTraceLvl_SClk, TRACELVL_NORMAL, "TS Diff:[%lld].[%f] - [%d]\n", 
				pTsCtx->tkDiff, 
				pTsCtx->usDiff,
				pTsCtx->tick); 
		++pTsCtx->tkDiff; 
	}
	return;
}

static 	void  	_STs_Init(_t_tsCtx * pTsCtx)
{
	pTsCtx->tick	= 0;	
	pTsCtx->reserv	= 0;
	pTsCtx->tkDiff	= 0;	
	pTsCtx->usDiff	= 0.0;	
	pTsCtx->curr.QuadPart=
	pTsCtx->last.QuadPart= 0;

	/* Calc the Freq & UsRatio */
/* Linux */
#if 	_TARGET_OS == _OS_GNU
	pTsCtx->freq.QuadPart = _NANO_US_VAL;
/* Windows */
#elif 	_TARGET_OS == _OS_WIN
	_TIMEFREQ_GET(&(pTsCtx->freq));
#endif
	pTsCtx->usRatio = (double)(1000000.0)/(double)(pTsCtx->freq.QuadPart);

	/* Get the TimeBase */
	_TIMEBASE_GET(&(pTsCtx->last));
	
	SYS_TRACE(_gTraceLvl_SClk, TRACELVL_NORMAL, "TS Init:Freq:[%llu]-[%f]-[%llu]\n\n", 
		pTsCtx->freq.QuadPart, pTsCtx->usRatio, pTsCtx->last.QuadPart); 
	
	return;
}


/* ---------------------------------------------------------------------------
**	Trigger OPs
*/
#define _STRIG_MAX 		0X10
	
typedef struct {
	_t_bnode			node;
	_t_cbentry 			entry;
}_t_tickCb;

typedef struct{
	/* Linux */
	_t_sysSemB 			wake;
	/* Windows */
#if 	_TARGET_OS == _OS_WIN
	_t_timestamp		ts;
	_t_sysHandle 		handle;
#endif
	_t_sysSemM			lock;
	_t_blist 			freeQ;
	_t_blist 			actvQ;
	_t_tickCb 			cbAU[_STRIG_MAX];
}_t_tickCtx;

static 	_t_tickCtx 			_gSTrig_Ctx;
static 	_t_tickCtx 			_gSClk_Ctx;

static 	void 	_STick_Init(_t_tickCtx * pCtx)
{
	_t_tickCb 	*	pCB;
	long 	i;

	if(pCtx){
		SYS_SEM_B_INIT(&(pCtx->wake));

		SYS_INIT_CS(&(pCtx->lock));
		BLIST_INIT(&(pCtx->freeQ));
		BLIST_INIT(&(pCtx->actvQ));
		pCB = pCtx->cbAU;
		for(i=0; i<_STRIG_MAX; i++, pCB++){
			BLST_ADD2T(&(pCtx->freeQ), &(pCB->node));
		}
	}
	return;
}

static 	void 	_STick_Stub(_t_tickCtx * pCtx){
	long 			i, count;
	_t_tickCb 	*	pCB;
	_t_blist 	* 	pLst;
	
	if(pCtx){
		/* Critical Section */
		SYS_ENTER_CS(&(pCtx->lock));

		pLst  = &(pCtx->actvQ);
		count = pLst->count;
		for(i=0, pCB = (_t_tickCb *)(pLst->next); 
			i<count; 
			i++, pCB = (_t_tickCb *)(pCB->node.next)){
			if(pCB->entry.cbProc){
				pCB->entry.cbProc(pCB->entry.pArg, pCB->entry.lArg);
			}
		}
		SYS_LEAVE_CS(&(pCtx->lock));
	}
	return;
}

static 	int 	_STick_Add(_t_tickCtx * pCtx, _t_cbentry * pEntry)
{
	int 			ret;
	long 			i, count;
	_t_tickCb 		*pCB, *pNext;
	_t_blist 		*pLst;

	ret 	= -1;
	if(pCtx && pEntry){
		/* Critical Section */
		SYS_ENTER_CS(&(pCtx->lock));

		/* Chk 4 Dup */
		pLst  = &(pCtx->actvQ);
		count = pLst->count;
		for(i=0, pCB = (_t_tickCb *)(pLst->next); 
			i<count; 
			i++, pCB = pNext){
			pNext = (_t_tickCb *)(pCB->node.next);
			if(	(pCB->entry.cbProc == pEntry->cbProc) &&
				(pCB->entry.pArg == pEntry->pArg) &&
				(pCB->entry.lArg == pEntry->lArg)){
				ret = 0;
				break;
			}
		}
		/* Add Entry */
		if(ret == -1){
			pCB = (_t_tickCb*)BLST_GET(&(pCtx->freeQ));
			if(pCB){
				_CBENTRY_ASSIGN(&(pCB->entry), pEntry);
				BLST_ADD2T(&(pCtx->actvQ), &(pCB->node));
				ret = 0;
			}
		}

		SYS_LEAVE_CS(&(pCtx->lock));
	}
	return ret;
}

static 	int 	_STick_Del(_t_tickCtx * pCtx, _t_cbentry * pEntry)
{
	int 			ret;
	long 			i, count;
	_t_tickCb 		*pCB, *pNext;
	_t_blist 		*pLst;
	
	ret 	= -1;
	if(pCtx && pEntry){
		/* Critical Section */
		SYS_ENTER_CS(&(pCtx->lock));

		/* Chk 4 Exist */
		pLst  = &(pCtx->actvQ);
		count = pLst->count;
		for(i=0, pCB = (_t_tickCb *)(pLst->next); 
			i<count; 
			i++, pCB = pNext){
			pNext = (_t_tickCb *)(pCB->node.next);
			if(	(pCB->entry.cbProc == pEntry->cbProc) &&
				(pCB->entry.pArg == pEntry->pArg) &&
				(pCB->entry.lArg == pEntry->lArg)){
				/* Del Entry */
				BLST_DEL(pLst, &(pCB->node));
				BLST_ADD2H(&(pCtx->freeQ), &(pCB->node));
				ret = 0;
			}
		}
		
		SYS_LEAVE_CS(&(pCtx->lock));
	}
	
	return ret;

}

int 		STrigger_Add(_t_cbentry * pEntry)
{
	return _STick_Add(&_gSTrig_Ctx, pEntry);
}

int 		STrigger_Del(_t_cbentry * pEntry)
{
	return _STick_Del(&_gSTrig_Ctx, pEntry);
}

int 		SClk_Add(_t_cbentry * pEntry)
{
	return _STick_Add(&_gSClk_Ctx, pEntry);
}

int 		SClk_Del(_t_cbentry * pEntry)
{
	return _STick_Del(&_gSClk_Ctx, pEntry);
}


#if 	_TARGET_OS == _OS_GNU
static 	void  	_STrigger_Exec(void){
	while(1){
		SYS_SEM_B_TAKE(&(_gSTrig_Ctx.wake), _FREQ_MS_STRIG*2);
		/* Process */
		_STick_Stub(&_gSTrig_Ctx);
	}
}

static 	void 	_STrigger_SigProc(int signo){
	switch (signo){
		case SIGALRM:
			SYS_SEM_B_GIVE(&(_gSTrig_Ctx.wake));
			break;
		default:
			break;
	}
	return;
}

static 	void 	_STrigger_Entry(void)
{
	struct  sigaction   vSigAct;    
	struct  itimerval   vTimer;    

	/* Init ITimer Sem & Sigaction */
  	sigemptyset(&vSigAct.sa_mask);    
  	vSigAct.sa_flags	= 0;    
  	vSigAct.sa_handler	= _STrigger_SigProc;    
  	sigaction(SIGALRM,&vSigAct,NULL);   

  	vTimer.it_value.tv_sec	= 0;    
  	vTimer.it_value.tv_usec	= _FREQ_MS_STRIG * 1000;    
  	vTimer.it_interval		= vTimer.it_value;    
  	if(setitimer(ITIMER_REAL,&vTimer,NULL) != 0){
		SYS_TRACE(_gTraceLvl_SClk, TRACELVL_ERROR, "TimeTrigger Create Fail!\n");
  	}
	
	_STrigger_Exec();
	return;
}


/* We'll Use the System Clock Service. */
#define SIGTIMER 			SIGRTMIN+1
static 	timer_t 			_gSClk_Posix;

static 	void 	_SClk_SigProc(int signo, siginfo_t * pInfo, void * pVoid){
	if(signo == SIGTIMER){
		_STs_Exec(&_gSTs_Ctx);
		SYS_SEM_B_GIVE(&(_gSClk_Ctx.wake));
	}
	return;
}

static 	void 	_SClk_Entry(void)
{
	struct 	sigaction 	vSigClk;
	struct 	sigevent 	vSigEv;
	struct 	itimerspec 	vITimer;
	struct 	itimerspec 	vOTimer;
	
	/* Init Clk Sigaction */
	sigemptyset(&vSigClk.sa_mask);
	vSigClk.sa_flags 		= SA_SIGINFO;
	vSigClk.sa_sigaction 	= _SClk_SigProc;
	sigaction(SIGTIMER, &vSigClk, NULL);

	/* Create the POSIX timer to generate signo */
	vSigEv.sigev_notify 	= SIGEV_SIGNAL;
	vSigEv.sigev_signo 	= SIGTIMER;
	vSigEv.sigev_value.sival_ptr = &_gSClk_Posix;
	vITimer.it_value.tv_sec 		= 0;
	vITimer.it_value.tv_nsec 		= _FREQ_MS_SCLK * (1000000L);
	vITimer.it_interval.tv_sec 		= vITimer.it_value.tv_sec;
	vITimer.it_interval.tv_nsec 	= vITimer.it_value.tv_nsec;

	if(	(timer_create(CLOCK_REALTIME, &vSigEv, &_gSClk_Posix) == 0) && 
		(timer_settime(_gSClk_Posix, 0, &vITimer, &vOTimer) == 0)) {
		while(1){
			SYS_SEM_B_TAKE(&(_gSClk_Ctx.wake), _WAIT_FOREVER);
			SYS_TRACE(_gTraceLvl_SClk, TRACELVL_DEBUG, "Clock Stub: %d - %ld\n",
				_gSTs_Ctx.tick, _gSTs_Ctx.tkDiff);
			/* Process */
			_STick_Stub(&_gSClk_Ctx);
		}
	}
	return;
}

void 	SClk_LibInit(void)
{
	/* Init the Trigger */
	_STick_Init(&_gSTrig_Ctx);
	_STick_Init(&_gSClk_Ctx);

	/* Init the TimeStamp */
	clock_gettime(CLOCK_REALTIME, &_gSTs_Base);
	_STs_Init(&_gSTs_Ctx);

	/* Create Task */
	STASK_CRATE(_gTask_SClk, 
					_STASK_STACK_CLK, 
					_STASK_PRI_CLK,
					_SClk_Entry, 
					NULL);

	STASK_CRATE(_gTask_STrig, 
					_STASK_STACK_TRIG, 
					_STASK_PRI_TRIG,
					_STrigger_Entry,
					NULL);
	return;
}


/* ----------	Target OS Windows	---------- */
#elif 	_TARGET_OS == _OS_WIN
static 	sInt32 				_gSClk_Res;

static 	void 	_STrigger_Exec(void){
	while(1){
		SetWaitableTimer(_gSTrig_Ctx.handle, &(_gSTrig_Ctx.ts), 0,  NULL,  NULL, FALSE);
		WaitForSingleObject(_gSTrig_Ctx.handle, _WAIT_FOREVER);
		/* Process */
		_STick_Stub(&_gSTrig_Ctx);
		
	}
	return;
}

static 	void 	_STrigger_Entry(void){
	_STrigger_Exec();
	return;
}

static 	void  SYS_CALLBACK	_SClk_SigProc(	UINT 	uTimerID, UINT 	uMsg, 
											DWORD_PTR 	dwUser, DWORD_PTR 	dw1, DWORD_PTR 	dw2){

	_STs_Exec(&_gSTs_Ctx);
	SYS_SEM_B_GIVE(&(_gSClk_Ctx.wake));
	return;
}

static 	void	_SClk_Entry(void){
	timeSetEvent(_FREQ_MS_SCLK, 
				_gSClk_Res, 
				_SClk_SigProc, 
				0, 
				TIME_PERIODIC);
	while(1){
		SYS_SEM_B_TAKE(&(_gSClk_Ctx.wake), _WAIT_FOREVER);
		SYS_TRACE(_gTraceLvl_SClk, TRACELVL_DEBUG, "Clock Stub: %d - %ld\n",
			_gSTs_Ctx.tick, _gSTs_Ctx.tkDiff);
		/* Process */
		_STick_Stub(&_gSClk_Ctx);
	}
}


void 	SClk_LibInit(void)
{
	TIMECAPS	tc;

	/* Set Process Priority */
#if 0
#define 	SCLK_PROC_PRI 		REALTIME_PRIORITY_CLASS
#else
#define 	SCLK_PROC_PRI 		HIGH_PRIORITY_CLASS
#endif
	SetPriorityClass(GetCurrentProcess(), SCLK_PROC_PRI);

	/* Set Clk Accuracy */
	timeGetDevCaps(&tc, sizeof(TIMECAPS));
	SYS_TRACE(_gTraceLvl_SClk, TRACELVL_DEBUG, "Clk Caps: Min[%d] Max[%d]\n", 
		tc.wPeriodMin, 	tc.wPeriodMax); 

	timeBeginPeriod(tc.wPeriodMin);	
	_gSClk_Res = tc.wPeriodMin;

	/* Init the Trigger */
	_STick_Init(&_gSTrig_Ctx);
	_STick_Init(&_gSClk_Ctx);

	/* Init the TimeStamp */
	_STs_Init(&_gSTs_Ctx);

	/* Create Task */
	STASK_CRATE(_gTask_SClk, 
				_STASK_STACK_CLK, 
				_STASK_PRI_CLK, 
				_SClk_Entry,
				NULL);

	_gSTrig_Ctx.handle = CreateWaitableTimer( NULL, FALSE, NULL);
	if(_gSTrig_Ctx.handle){
		_gSTrig_Ctx.ts.QuadPart = -(_FREQ_MS_STRIG * 10000); 
	}else{
		SYS_TRACE(_gTraceLvl_SClk, TRACELVL_ERROR, "TimeTrigger Create Fail!\n");
	}
	STASK_CRATE(_gTask_STrig, 
				_STASK_STACK_TRIG, 
				_STASK_PRI_TRIG, 
				_STrigger_Entry, 
				NULL);
	return;
}


#endif




